////////////////////////////////////////////////////////////////////////////////////
/* Program: Target Processing Layer of SENTINEL Project
 * Author:	Eric Cleveland
 * Editors: Sean Pierce, Robert Wunderlich, Ryan Bell, (Team PAINTeK)
 * Purpose: Image Processing necessary for the SENTINEL system to determine motion
 *			and targets within the field of view.
 * Misc:	Test
 */

/* *************** License:**************************
   Oct. 3, 2008
   Right to use this code in any way you want without warrenty, support or any guarentee of it working.

   BOOK: 
   Learning OpenCV: Computer Vision with the OpenCV Library
     by Gary Bradski and Adrian Kaehler
     Published by O'Reilly Media, October 3, 2008
     ISBN-10: 0596516134 or: ISBN-13: 978-0596516130    
************************************************** */
#include <winsock2.h>
#include <ws2tcpip.h>
#include <stdio.h>
#include <ctype.h>
#include <iostream>

//OpenCV Libraries
#include <opencv2/video/background_segm.hpp>
#include <opencv2/imgproc/imgproc_c.h>
#include <opencv2/highgui/highgui.hpp>

//Sockets
#define SEND_PORT "5566"
#pragma comment(lib, "Ws2_32.lib")

//VARIABLES for CODEBOOK METHOD:
CvBGCodeBookModel* model = 0;
const int NCHANNELS = 3;
bool ch[NCHANNELS]={true,true,true}; // This sets what channels should be adjusted for background bounds


int main(int argc, char** argv)
{
    const char* filename = 0;
    IplImage* rawImage = 0, *yuvImage = 0; //yuvImage is for codebook method
    IplImage *ImaskCodeBook = 0,*ImaskCodeBookCC = 0;
	//Socket Definition, Image storage
	const char* IPAddress = "127.0.0.1"; //Local Host
	IplImage* contourImage = 0; //Contour image set to null
	CvSeq* contours = 0; //Memory for CV Sequential
	CvMemStorage* passingStorage = NULL; //frame differencing passing storage
	CvRect targetHighlight; // Bounding rectangle for use of finding coordinates
	int Centerx, Centery; // Useful ints to pass target coordinates
	///////////////
    CvCapture* capture = 0; // Capture camera object

    int c, n, nframes = 0;
    int nframesToLearnBG = 300; //Gather 300 frames to learn background << Performance bottleneck

    model = cvCreateBGCodeBookModel();
    
    //Set color thresholds to default values
    model->modMin[0] = 3;
    model->modMin[1] = model->modMin[2] = 3;
    model->modMax[0] = 10;
    model->modMax[1] = model->modMax[2] = 10;
    model->cbBounds[0] = model->cbBounds[1] = model->cbBounds[2] = 10;

    bool pause = false;
    bool singlestep = false;

	
    for( n = 1; n < argc; n++ )
    {
        static const char* nframesOpt = "--nframes=";
        if( strncmp(argv[n], nframesOpt, strlen(nframesOpt))==0 )
        {
            if( sscanf(argv[n] + strlen(nframesOpt), "%d", &nframesToLearnBG) == 0 )
            {           
                return -1;
            }
        }
        else
            filename = argv[n];
    }

    if( !filename )
    { //Debugging carried out through AvI
        printf("Capture from camera\n");
        capture = cvCaptureFromCAM( 0 );
    }
    else //Useless code
    {
        printf("Capture from file %s\n",filename);
        capture = cvCreateFileCapture( filename );
    }

    if( !capture )
    {
        printf( "Can not initialize video capturing\n\n" );
        return -1;
    }
	////////////////////
	//Communication Initialization
	WSADATA wsadata;
	int error = WSAStartup(MAKEWORD(2,2), &wsadata);
	if(error)
	{
		printf("Socket Communication fail code: 1\n");
	}
	if(wsadata.wVersion != 0x0202)
	{
		WSACleanup();
		printf("Socket Communication fail code: 2\n");
	}

	//intitialize the socket
	struct addrinfo *result = NULL,
					*ptr = NULL,
					hints;

	ZeroMemory(&hints, sizeof(hints));
	hints.ai_family = AF_UNSPEC;
	hints.ai_socktype = SOCK_DGRAM;
	hints.ai_protocol = IPPROTO_UDP;

	error = getaddrinfo(IPAddress, SEND_PORT, &hints, &result);
	if(error != 0)
	{
		printf("Socket Communication fail code: 3\n");
		WSACleanup();
	}

	SOCKET ConnectSocket = INVALID_SOCKET;
	
	// Attempt to connect to the first address returned by
	// the call to getaddrinfo
	ptr = result;

	// Create a SOCKET for connecting to server
	ConnectSocket = socket(ptr->ai_family, ptr->ai_socktype, ptr->ai_protocol);

	if(ConnectSocket == INVALID_SOCKET)
	{
		printf("Error at socket(): ", WSAGetLastError(),"\n");
		freeaddrinfo(result);
		WSACleanup();
	}
	// Connect to server
	error = connect(ConnectSocket, ptr->ai_addr, (int)ptr->ai_addrlen);
	if(error == SOCKET_ERROR)
	{
		closesocket(ConnectSocket);
		ConnectSocket = INVALID_SOCKET;
	}

	
	// Should really try the next address returned by getaddrinfo
	// if the connect call failed
	// But for this simple example we just free the resources
	// returned by getaddrinfo and print an error message

	freeaddrinfo(result);

	if(ConnectSocket == INVALID_SOCKET)
	{
		printf("Unable to connect to server!\n");
		WSACleanup();
	}
	char *sendbuf = new char[10];
	////////////
    //MAIN PROCESSING LOOP:
    for(;;)
    {
        if( !pause )
        {
            rawImage = cvQueryFrame( capture );
            ++nframes;
            if(!rawImage) 
                break;
        }
        if( singlestep )
            pause = true;
        
        //First time:
        if( nframes == 1 && rawImage )
        {
            // CODEBOOK METHOD ALLOCATION
            yuvImage = cvCloneImage(rawImage);
            ImaskCodeBook = cvCreateImage( cvGetSize(rawImage), IPL_DEPTH_8U, 1 );
            ImaskCodeBookCC = cvCreateImage( cvGetSize(rawImage), IPL_DEPTH_8U, 1 );
			contourImage = cvCreateImage( cvGetSize(rawImage), IPL_DEPTH_8U, 1 );
			passingStorage = cvCreateMemStorage(0);
            cvSet(ImaskCodeBook,cvScalar(255));
            
            cvNamedWindow( "Raw", 1 );
            cvNamedWindow( "ForegroundCodeBook",1);
            cvNamedWindow( "CodeBook_ConnectComp",1);
			cvNamedWindow( "Contours",1);
        }

        // If we've got an rawImage and are good to go:                
        if( rawImage )
        {
            cvCvtColor( rawImage, yuvImage, CV_BGR2YCrCb );//YUV For codebook method
            //This is where we build our background model
            if( !pause && nframes-1 < nframesToLearnBG  )
                cvBGCodeBookUpdate( model, yuvImage );

            if( nframes-1 == nframesToLearnBG  )
                cvBGCodeBookClearStale( model, model->t/2 );
            
            //Find the foreground if any
            if( nframes-1 >= nframesToLearnBG  )
            {
                // Find foreground by codebook method
                cvBGCodeBookDiff( model, yuvImage, ImaskCodeBook );
                // This part just to visualize bounding boxes and centers if desired
                cvCopy(ImaskCodeBook,ImaskCodeBookCC);	
                cvSegmentFGMask( ImaskCodeBookCC );
				//ERIC
				cvCopy(ImaskCodeBookCC, contourImage);
				cvFindContours(contourImage, passingStorage, &contours);
				cvZero(contourImage);
				if(contours)
				{
					cvDrawContours(contourImage, contours, cvScalarAll(200), cvScalarAll(255), 100);
					targetHighlight = cvBoundingRect(contours, 0);
					Centerx = (targetHighlight.x+(targetHighlight.x+targetHighlight.width))/2;
					Centery = (targetHighlight.y+(targetHighlight.y+targetHighlight.height))/2;
					std::cout << "Coordinate: " << targetHighlight.x << " , " << targetHighlight.y << "\n";
					//SENDBUFFER
					cvRectangle(rawImage, cvPoint(targetHighlight.x, targetHighlight.y), cvPoint(targetHighlight.x+targetHighlight.width, targetHighlight.y+targetHighlight.height) , CV_RGB(0,255,0), 5, 8, 0);
					// send an initial buffer
					sprintf(sendbuf, "%d,%d,0", Centerx, Centery);
					std::cout << "SEND BUFFER " << sendbuf;
					error = send(ConnectSocket, sendbuf, (int) strlen(sendbuf)+1, 0);
					if(error == SOCKET_ERROR)
					{
						std::cout << "send failed: " << WSAGetLastError() << "\n";
						closesocket(ConnectSocket);
						WSACleanup();
						return false;
					}
				}
				// /ERIC
            }
            //Display
            cvShowImage( "Raw", rawImage );
            cvShowImage( "ForegroundCodeBook",ImaskCodeBook);
            cvShowImage( "CodeBook_ConnectComp",ImaskCodeBookCC);
			//ERIC
			cvShowImage( "Contours", contourImage);
			// /ERIC
        }

        // User input:
        c = cvWaitKey(10)&0xFF;
        c = tolower(c);
        // End processing on ESC, q or Q
        if(c == 27 || c == 'q')
            break;
        //Else check for user input
        switch( c )
        {
        case 'p':
            pause = !pause;
            break;
        case 's':
            singlestep = !singlestep;
            pause = false;
            break;
        case 'r':
            pause = false;
            singlestep = false;
            break;
        case ' ':
            cvBGCodeBookClearStale( model, 0 );
            nframes = 0;
            break;
            //CODEBOOK PARAMS
        case 'y': case '0':
        case 'u': case '1':
        case 'v': case '2':
        case 'a': case '3':
        case 'b': 
            ch[0] = c == 'y' || c == '0' || c == 'a' || c == '3';
            ch[1] = c == 'u' || c == '1' || c == 'a' || c == '3' || c == 'b';
            ch[2] = c == 'v' || c == '2' || c == 'a' || c == '3' || c == 'b';
            printf("CodeBook YUV Channels active: %d, %d, %d\n", ch[0], ch[1], ch[2] );
            break;
        case 'i': //modify max classification bounds (max bound goes higher)
        case 'o': //modify max classification bounds (max bound goes lower)
        case 'k': //modify min classification bounds (min bound goes lower)
        case 'l': //modify min classification bounds (min bound goes higher)
            {
            uchar* ptr = c == 'i' || c == 'o' ? model->modMax : model->modMin;
            for(n=0; n<NCHANNELS; n++)
            {
                if( ch[n] )
                {
                    int v = ptr[n] + (c == 'i' || c == 'l' ? 1 : -1);
                    ptr[n] = cv::saturate_cast<uchar>(v);
                }
                printf("%d,", ptr[n]);
            }
            printf(" CodeBook %s Side\n", c == 'i' || c == 'o' ? "High" : "Low" );
            }
            break;
        }
    }		
    
    cvReleaseCapture( &capture );
    cvDestroyWindow( "Raw" );
    cvDestroyWindow( "ForegroundCodeBook");
    cvDestroyWindow( "CodeBook_ConnectComp");
	cvDestroyWindow( "Contours" );
	closesocket(ConnectSocket);
	WSACleanup();
    return 0;
}